#include <iostream>

#include "IK_xbk.h"

using namespace std;
using namespace Eigen;

//ur5数据
// double d[6 + 1] = { 0, 0.089159,0,0,0.10915,0.09465,0.0823 };
// double a[6] = { 0,-0.42500,-0.39225,0,0,0 };
// double alpha[6] = { 1.570796, 0, 0, 1.570796, -1.570796, 0 };
// double theta[8 + 1][6 + 1];//八组解

double d[6 + 1] = { 0, 0.077,0,0,0.043,0.073,0.001 };//zui hou yi ge shu xu yao zui zhong que ding
double a[6] = { 0,-0.143,-0.143,0,0,0 };
double alpha[6] = { 1.570796, 0, 0, 1.570796, -1.570796, 0 };
double theta[8 + 1][6 + 1];//八组解

//正运动学
Mat44 kinematics(Mat61 theta_in)
{
	double theta_input[7];
	for (int i = 0; i < 6; i++)
	{
		theta_input[i+1] = theta_in[i];
	}

	Eigen::Matrix4d T[6 + 1];//为了和theta对应，0不用
	for (int i = 1; i <= 6; i++)
	{

		//theta_input[i] = theta_input[i] * 3.1415926/180;
		//cout << "   alpha=" << alpha[i-1] << "     a0=" << a[1] << endl;
		T[i](0, 0) = cos(theta_input[i]);
		T[i](0, 1) = -sin(theta_input[i]) * cos(alpha[i - 1]);
		T[i](0, 2) = sin(theta_input[i]) * sin(alpha[i - 1]);
		T[i](0, 3) = a[i - 1] * cos(theta_input[i]);
		T[i](1, 0) = sin(theta_input[i]);
		T[i](1, 1) = cos(theta_input[i]) * cos(alpha[i - 1]);
		T[i](1, 2) = -cos(theta_input[i]) * sin(alpha[i - 1]);
		T[i](1, 3) = a[i - 1] * sin(theta_input[i]);
		T[i](2, 0) = 0;
		T[i](2, 1) = sin(alpha[i - 1]);
		T[i](2, 2) = cos(alpha[i - 1]);
		T[i](2, 3) = d[i];
		T[i](3, 0) = 0;
		T[i](3, 1) = 0;
		T[i](3, 2) = 0;
		T[i](3, 3) = 1;
	}
	Eigen::Matrix4d T06 = T[1] * T[2] * T[3] * T[4] * T[5] * T[6];
	return T06;
}
//逆运动学
Mat86 Inverse_kinematics(Matrix4d T06)
{
	
	double A, B, C, D, E, F, G, M, N;//用大写字母替代常数

//theta1 两个解
	A = d[6] * T06(1, 2) - T06(1, 3);
	B = d[6] * T06(0, 2) - T06(0, 3);
	C = d[4];
	//第一个解，赋给一到四组
	theta[1][1] = atan2(A, B) - atan2(C, sqrt(A * A + B * B - C * C));
	theta[2][1] = theta[1][1];
	theta[3][1] = theta[1][1];
	theta[4][1] = theta[1][1];
	//第二个解，赋给五到八组
	theta[5][1] = atan2(A, B) - atan2(C, -sqrt(A * A + B * B - C * C));
	theta[6][1] = theta[5][1];
	theta[7][1] = theta[5][1];
	theta[8][1] = theta[5][1];

	//theta5 四个解
		//由theta[1][1]产生的第一个解，赋给一到二组
	A = sin(theta[1][1]) * T06(0, 2) - cos(theta[1][1]) * T06(1, 2);
	theta[1][5] = acos(A);
	theta[2][5] = theta[1][5];
	//由theta[1][1]产生的第二个解，赋给三到四组
	theta[3][5] = -acos(A);
	theta[4][5] = theta[3][5];
	//由theta[5][1]产生的第一个解，赋给五到六组
	A = sin(theta[5][1]) * T06(0, 2) - cos(theta[5][1]) * T06(1, 2);
	theta[5][5] = acos(A);
	theta[6][5] = theta[5][5];
	//由theta[5][1]产生的第二个解，赋给七到八组
	theta[7][5] = -acos(A);
	theta[8][5] = theta[7][5];

	//theta6 四个解
	for (int i = 1; i <= 8; i = i + 2)
	{
		A = (sin(theta[i][1]) * T06(0, 0) - cos(theta[i][1]) * T06(1, 0));
		B = (sin(theta[i][1]) * T06(0, 1) - cos(theta[i][1]) * T06(1, 1));
		C = sin(theta[i][5]);
		D = A * A + B * B - C * C;

		if ((C <= -0.00001) || (C >= 0.000001)) {
			theta[i][6] = atan2(A, B) - atan2(C, 0.00);
			theta[i + 1][6] = atan2(A, B) - atan2(C, 0.00);
		}
		else
		{
			theta[i][6] = 0;
			theta[i + 1][6] = 0;
		}

	}

	//theta3 8组解
	for (int i = 1; i <= 8; i = i + 2)
	{
		C = cos(theta[i][1]) * T06(0, 0) + sin(theta[i][1]) * T06(1, 0);
		D = cos(theta[i][1]) * T06(0, 1) + sin(theta[i][1]) * T06(1, 1);
		E = cos(theta[i][1]) * T06(0, 2) + sin(theta[i][1]) * T06(1, 2);
		F = cos(theta[i][1]) * T06(0, 3) + sin(theta[i][1]) * T06(1, 3);
		G = cos(theta[i][6]) * T06(2, 1) + sin(theta[i][6]) * T06(2, 0);
		A = d[5] * (sin(theta[i][6]) * C + cos(theta[i][6]) * D) - d[6] * E + F;
		B = T06(2, 3) - d[1] - T06(2, 2) * d[6] + d[5] * G;


		//theta3
		if (A * A + B * B <= (a[2 - 1] + a[3 - 1]) * (a[2 - 1] + a[3 - 1])) {
			theta[i][3] = acos((A * A + B * B - a[2 - 1] * a[2 - 1] - a[3 - 1] * a[3 - 1]) / (2 * a[2 - 1] * a[3 - 1]));
			theta[i + 1][3] = -theta[i][3];
		}
		else
		{
			theta[i][3] = 0;
			theta[i + 1][3] = 0;
		}
	}

	//theta2 theta4
	for (int i = 1; i <= 8; i++)
	{
		C = cos(theta[i][1]) * T06(0, 0) + sin(theta[i][1]) * T06(1, 0);
		D = cos(theta[i][1]) * T06(0, 1) + sin(theta[i][1]) * T06(1, 1);
		E = cos(theta[i][1]) * T06(0, 2) + sin(theta[i][1]) * T06(1, 2);
		F = cos(theta[i][1]) * T06(0, 3) + sin(theta[i][1]) * T06(1, 3);
		G = cos(theta[i][6]) * T06(2, 1) + sin(theta[i][6]) * T06(2, 0);
		A = d[5] * (sin(theta[i][6]) * C + cos(theta[i][6]) * D) - d[6] * E + F;
		B = T06(2, 3) - d[1] - T06(2, 2) * d[6] + d[5] * G;

		M = ((a[3 - 1] * cos(theta[i][3]) + a[2 - 1]) * B - a[3 - 1] * sin(theta[i][3]) * A) / (a[2 - 1] * a[2 - 1] + a[3 - 1] * a[3 - 1] + 2 * a[2 - 1] * a[3 - 1] * cos(theta[i][3]));
		N = (A + a[3 - 1] * sin(theta[i][3]) * M) / (a[3 - 1] * cos(theta[i][3]) + a[2 - 1]);
		theta[i][2] = atan2(M, N);

		//theta4
		theta[i][4] = atan2((-sin(theta[i][6]) * C - cos(theta[i][6]) * D), G) - theta[i][2] - theta[i][3];
	}

	Mat86 ret;	

	
	for (int i = 1; i < 9; i++)
	{
		for (int j = 1; j < 7; j++)
		{
			ret(i-1,j-1) =  theta[i][j];
		}
		
	}

	return ret;
}

